from queue import Queue
from functools import wraps
import mvsdk
from mvsdk import tSdkImageResolution
from math import atan, pi, sqrt, pow, sin, cos
import time
import cv2
import numpy as np
import traceback


class Debug:

    def __init__(self, Queues: Queue, timeSwitch: bool = False, debugName: str = 'System',
                 timeFormat: str = '%H:%M:%S'):
        self.debugName = debugName
        self.timeFormat = timeFormat
        self.__timeSwitch = timeSwitch
        self.__Q = Queues

    def pushInfo(self, strs: str, status: str = 'Debug'):
        if not self.__timeSwitch:
            infoText = '[{}-{}]--> {}'.format(self.debugName, status, strs)
        else:
            infoText = '[{}-{}-{}]--> {}'.format(self.debugName,
                                                 time.strftime(self.timeFormat, time.localtime(time.time())),
                                                 status, strs)
        if not self.__Q.full():
            self.__Q.put(infoText)
            print(infoText)
        else:
            self.__Q.get()
            self.__Q.put(infoText)
            print(infoText)

    def getInfo(self):
        if not self.__Q.empty():
            res = self.__Q.get()
            return res
        else:
            return None


class CameraError(Exception):

    def __init__(self, string: str):
        self.string = str

    def __str__(self):
        print(self.string)


def decorate_break(func):
    @wraps(func)
    def infunc(self, *args, **kwargs):
        if self.videoPath is None:
            devhandle = self._devHandle
            mvsdk.CameraPause(devhandle)
            func(self, *args, **kwargs)
            mvsdk.CameraPlay(devhandle)
        else:
            func(self, *args, **kwargs)

    return infunc


class Camera:

    def __init__(self, videoPath=None):
        self.videoPath = videoPath
        if self.videoPath is None:
            devList = mvsdk.CameraEnumerateDevice()
            # print(devList)
            if len(devList) < 1:
                raise CameraError('[CameraPacket]: No camera as found!')
            # 获取加载到的设备， 如果没找到就升起错误
            self._dev = mvsdk
            self._devInfo = devList[0]
            self._isOpen_ = False
            # 获取设备的信息，使状态为关闭

            try:
                self._devHandle = mvsdk.CameraInit(self._devInfo, -1, -1)
            except:
                self._devHandle = 1
            print(self._devHandle)
            self._devCapability = None
            self._devFps = 0
            self._devFrameSize = 0
            self._devFrameMode = None
            self._devFrameHeight = 0
            self._devFrameWidth = 0
            self._devFrameSpace = None
            self._isPlay_ = 0
        else:
            self.videoPath = videoPath
            self._dev = cv2.VideoCapture(videoPath)
        # 以上为设备基本所用变量

    def open(self):
        if self.videoPath is None:
            if not self._isOpen_:
                self._isOpen_ = True

                # 对设备进行初始化操作
                mvsdk.CameraSetFrameSpeed(self._devHandle, 1)
                # 设置设备的帧率默认为mode1(0-mode0-低, 1-mode1-高)
                mvsdk.CameraRstTimeStamp(self._devHandle)
                # 使设备的图像采集时间戳重置
                self._devCapability = mvsdk.CameraGetCapability(self._devHandle)
                # 获得设备的特性描述变量
                monoCamera = (self._devCapability.sIspCapacity.bMonoSensor != 0)
                if monoCamera:
                    mvsdk.CameraSetIspOutFormat(self._devHandle, mvsdk.CAMERA_MEDIA_TYPE_MONO8)
                else:
                    mvsdk.CameraSetIspOutFormat(self._devHandle, mvsdk.CAMERA_MEDIA_TYPE_BGR8)
                # 判断设备是否为黑白相机，不是则设置设备ISP图像输出格式为BGR8
                mvsdk.CameraSetTriggerMode(self._devHandle, 0)
                # 设置设备图像采集模式为连续采集(0-连续, 1-软触, 2-硬触)
                mvsdk.CameraSetAeState(self._devHandle, 0)
                # 设置设备的曝光模式为手动(0-手动, 1-自动)
                mvsdk.CameraSetExposureTime(self._devHandle, 13.05 * 1000)
                # 设置设备的曝光时间为13.05ms
                mvsdk.CameraSetWbMode(self._devHandle, 1)
                mvsdk.CameraSetClrTempMode(self._devHandle, 1)
                mvsdk.CameraSetPresetClrTemp(self._devHandle, 2)
                # 设置设备色温为自动模式，
                self._devFrameWidth = self._devCapability.sResolutionRange.iWidthMax
                self._devFrameHeight = self._devCapability.sResolutionRange.iHeightMax
                self._devFrameMode = (1 if monoCamera else 3)
                # 获得设备图像的宽高和颜色层
                self._devFrameSize = self._devFrameWidth * self._devFrameHeight * self._devFrameMode
                self._devFrameSpace = mvsdk.CameraAlignMalloc(self._devFrameSize, 16)
                # 算出图像帧的大小并且开辟一个存储图像帧的空间
            else:
                raise CameraError("[CameraPacket]: CameraPacket Already Open!")
        else:
            pass

    def play(self):
        if self.videoPath is None:
            if self._isOpen_ and self._isPlay_ == 0:
                mvsdk.CameraPlay(self._devHandle)
                self._isPlay_ = 1
            else:
                print("[CameraPacket]: CameraPacket Not Open or Playing!")
        else:
            pass
        # 开启传输图像

    def frame(self, waitTime=1000, resolution=None):
        if self.videoPath is None:
            if self._isPlay_ == 1:
                try:
                    pRawData, FrameHead = mvsdk.CameraGetImageBuffer(self._devHandle, waitTime)
                    mvsdk.CameraImageProcess(self._devHandle, pRawData, self._devFrameSpace, FrameHead)
                    mvsdk.CameraReleaseImageBuffer(self._devHandle, pRawData)
                    frame_data = (mvsdk.c_ubyte * FrameHead.uBytes).from_address(self._devFrameSpace)
                    frame = np.frombuffer(frame_data, dtype=np.uint8)
                    frame = frame.reshape((FrameHead.iHeight, FrameHead.iWidth,
                                           1 if FrameHead.uiMediaType == mvsdk.CAMERA_MEDIA_TYPE_MONO8 else 3))
                    if resolution != None:
                        frame = cv2.resize(frame, resolution, interpolation=cv2.INTER_LINEAR)
                    return frame
                except mvsdk.CameraException as e:
                    if e.error_code != mvsdk.CAMERA_STATUS_TIME_OUT:
                        print("[DevError]: CameraGetImageBuffer failed({}): {}".format(e.error_code, e.message))
            else:
                raise CameraError("[CameraPacket]: CameraPacket now status is Stop!")
        else:
            _, frame = self._dev.read()
            return frame

    def stop(self):
        if self.videoPath is None:
            if self._isPlay_ == 1:
                mvsdk.CameraPause(self._devHandle)
                self._isPlay_ = 0
            else:
                print("[CameraPacket]: CameraPacket now status is Not Open or Stop!")
        else:
            pass

    def close(self):
        if self.videoPath is None:
            if self._isOpen_:
                mvsdk.CameraStop(self._devHandle)
                mvsdk.CameraUnInit(self._devHandle)
                mvsdk.CameraAlignFree(self._devFrameSpace)
                self._isOpen_ = False
                self._isPlay_ = 0
            else:
                raise CameraError("[CameraPacket]: CameraPacket Already Close!")
        else:
            pass
        # 关闭设备

    @property
    def getExposureTimeRange(self) -> tuple:
        """
        描述：得到曝光时间范围
        功能：获得相机的曝光时间范围
        参数：无
        返回值：(Min: 最小值, Max: 最大值, Step: 步进值) 单位ms
        """
        if self.videoPath is None:
            fMin, fMax, fStep = mvsdk.CameraGetExposureTimeRange(self._devHandle)
            return round(fMin / 1000, 4), round(fMax / 1000, 4), round(fStep / 1000, 4)
        else:
            return 1, 100, 1

    @decorate_break
    def setExposureTime(self, time: float):
        """
        @time: 曝光时间ms
        """
        if self.videoPath is None:
            mvsdk.CameraSetExposureTime(self._devHandle, time * 1000)

    @property
    def getExposureTime(self) -> int:
        """
        -> 曝光时间ms
        """
        if self.videoPath is None:
            return mvsdk.CameraGetExposureTime(self._devHandle) / 1000
        else:
            return 12

    @decorate_break
    def setFrameSpeedMode(self, mode: int):
        """
        @mode: 0为低速率, 1为高速率
        """
        if self.videoPath is None:
            mvsdk.CameraSetFrameSpeed(self._devHandle, mode)
        else:
            pass

    @decorate_break
    def setExposureGain(self, num: int):
        """
        @num: 增益值
        """
        if self.videoPath is None:
            mvsdk.CameraSetAnalogGain(self._devHandle, num)

    @decorate_break
    def setLutMode(self, emLutMode: int):
        """
        @emLutMode: 0为调节模式, 1为预设模式, 2为自定义表
        """
        if self.videoPath is None:
            mvsdk.CameraSetLutMode(self._devHandle, emLutMode)

    @decorate_break
    def setGamma(self, num: int):
        """
        @num: Gamma值的大小
        """
        if self.videoPath is None:
            mvsdk.CameraSetGamma(self._devHandle, num)

    @decorate_break
    def setContrast(self, num: int):
        """
        @num: 对比度的大小
        """
        if self.videoPath is None:
            mvsdk.CameraSetContrast(self._devHandle, num)

    @property
    def frameticks(self):
        if self._isPlay_ == 1:
            ticks = mvsdk.CameraGetFrameTimeStamp(self._devHandle)
            return ticks
        else:
            print("[CameraPacket]: CameraPacket now status is Stop!")

    @property
    def cameraImageResolution(self):
        """
        描述：相机图像分辨率
        功能：获得相机硬件输出的图像分辨率
        参数：无
        返回值：无
        """
        tsdkImageResolution = mvsdk.CameraGetImageResolution(self._devHandle)
        return tsdkImageResolution

    def setROI(self, offset: tuple, resolution: tuple):
        """
        描述：设置相机分辨率
        功能：设置相机硬件输出的图像分辨率或者ROI等信息
        参数：resolution: 分辨率元祖
        返回值：无
        """
        tsdkImageResolution = tSdkImageResolution()
        tsdkImageResolution.iIndex = 0xFF
        tsdkImageResolution.iHOffsetFOV = offset[0]
        tsdkImageResolution.iVOffsetFOV = offset[1]
        tsdkImageResolution.iWidthFOV = resolution[0]
        tsdkImageResolution.iHeightFOV = resolution[1]
        tsdkImageResolution.iWidth = resolution[0]
        tsdkImageResolution.iHeight = resolution[1]
        mvsdk.CameraSetImageResolution(self._devHandle, tsdkImageResolution)


if __name__ == '__main__':
    dev = Camera()
    dev.open()
    print(dev.cameraImageResolution)
    # dev.setGamma(50)
    dev.setExposureTime(15)
    dev.setExposureGain(0)
    dev.play()
    frameCenter = int(1280 / 2), int(1024 / 2)
    thelta = 57.652
    # width = 33.837
    width = 0
    globalxIndent = width * cos(pi * thelta / 180)
    globalyIndent = width * sin(pi * thelta / 180)
    while (cv2.waitKey(1) & 0xFF) != ord('q'):
        try:
            lastTime = time.time()
            frameRaw = dev.frame()
            # print(frameRaw.shape)
            # print(type(frameRaw))
            nowTime = time.time()
            frameB, frameG, frameR = cv2.split(frameRaw)
            frameSplit = cv2.subtract(frameR, frameB)
            frameThresh = cv2.threshold(frameSplit, 55, 255, cv2.THRESH_BINARY)[1]
            kernel_frame = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))
            frameOpen = cv2.morphologyEx(frameThresh, cv2.MORPH_CLOSE, kernel_frame)
            # # frame = cv2.flip(frameRaw, 1)
            # # cv2.imshow("Press q to end", frameRaw)
            frameCon, frameHie = cv2.findContours(frameOpen, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
            if len(frameCon) == 0:
                continue
            areaIndex = 0
            areaSize = 0
            for index in range(len(frameCon)):
                size = cv2.contourArea(frameCon[index])
                if size > areaSize:
                    areaSize = size
                    areaIndex = index
            if frameCon[areaIndex].size < 12:
                continue
            ellipseBox = cv2.fitEllipse(frameCon[areaIndex])
            # print(ellipseBox)
            frameResult = frameRaw.copy()
            ellipseX, ellipseY = int(ellipseBox[0][0]), int(ellipseBox[0][1])
            k = (frameCenter[1] - ellipseY) / (frameCenter[0] - ellipseX)
            width = sqrt(pow(frameCenter[0] - ellipseX, 2) + pow(frameCenter[1] - ellipseY, 2))
            print((atan(k) * 180 / pi, width))
            cv2.line(frameResult, (ellipseX, ellipseY), frameCenter, (255, 255, 0), 3)
            cv2.imshow("prees q to end2", frameResult)

            # new_x, new_y = int(frameCenter[0] + globalxIndent), int(frameCenter[1] + globalyIndent)
            # cv2.circle(frameRaw, frameCenter, 3, (255, 0, 0), 3)
            # cv2.circle(frameRaw, (new_x, new_y), 3, (255, 255, 255), 3)
            # cv2.imshow('test', frameOpen)

            # print("FPS {}".format(int(1 / (nowTime - lastTime))))
        except Exception as e:
            print("出现错误！", traceback.format_exc())
            break
    dev.close()
